/* ----------------------------------------------------------------------
*
* Copyright (C) 2024 XiaoFei. All rights reserved.
*
* @file:        XiaoFei_Matrix.c
* @author:      XiaoFei
* @Date:        28-October-2024
* @Revision:    V1
* @Project:     XiaoFei DSP Library
*
* -------------------------------------------------------------------- */

#include "XiaoFei_Matrix.h"
#include "stdlib.h"

#ifndef My_ABS
#define My_ABS(x)   (x<0) ? -(x) : (x)
#endif

/*
* @function:    My_MatrixCreate
* @parameter:   [in]    *rows           The rows of the matrix.
* @parameter:   [in]    *input          The cols of the matrix.
* @return:      The matrix memory applied to storage vectors.
*/
Matrix_TypeDef My_MatrixCreate(unsigned int rows, unsigned int cols, double* data) 
{  
    Matrix_TypeDef mat;
    mat.Rows=rows;  
    mat.Cols=cols;  
    mat.Val=(double**)malloc(rows* sizeof(double*));  
    for(int i=0;i<rows;i++)
        mat.Val[i]=(double*)malloc(cols*sizeof(double));  
    for(unsigned int i=0;i<rows;i++)
        for(unsigned int j=0;j<cols;j++)
            mat.Val[i][j]=data[i*rows+j];
    return mat;  
}

/*
* @function: 	My_MatrixFree
* @parameter:	[in] 	*mat			The first address of the input matrix.
* @return:		None.
*/
void My_MatrixFree(Matrix_TypeDef* mat) 
{  
	for(int i=0;i<mat->Rows;i++)
		free(mat->Val[i]);  
	free(mat->Val);  
}

/*
* @function: 	My_MatrixAdd
* @parameter:	[in]    *inputa			The first address of the input matrix.
* @parameter:	[in]    *inputb			The first address of another input matrix.
* @parameter:	[out]   *output			The first address of the output matrix based on matrix additions.
* @return:		Successes or failures of the results of the calculations.
*/
ResStatus_TypeDef My_MatrixAdd(Matrix_TypeDef* inputa, Matrix_TypeDef* inputb, Matrix_TypeDef* output)  
{  
	if(inputa->Rows!= inputb->Rows||inputa->Cols!=inputb->Cols)
		return Error;  
	output->Rows=inputa->Rows;  
	output->Cols=inputa->Cols;  
	output->Val=(double**)malloc(output->Rows*sizeof(double*));    
	for(unsigned int i=0;i<output->Rows;i++)  
		output->Val[i]=(double*)malloc(output->Cols*sizeof(double));    
	for(unsigned int i=0;i<inputa->Rows;i++)
		for(unsigned int j=0; j<inputa->Cols;j++)
			output->Val[i][j]=inputa->Val[i][j]+inputb->Val[i][j];
	return Success;  
}

/*
* @function: 	My_MatrixSub
* @parameter:	[in]    *inputa			The first address of the input matrix.
* @parameter:	[in]    *inputb			The first address of another input matrix.
* @parameter:	[out]   *output			The first address of the output matrix based on matrix subtraction.
* @return:		Successes or failures of the results of the calculations.
*/
ResStatus_TypeDef My_MatrixSub(Matrix_TypeDef* inputa, Matrix_TypeDef* inputb, Matrix_TypeDef* output)  
{  
	if(inputa->Rows!= inputb->Rows||inputa->Cols!=inputb->Cols)
		return Error;  
	output->Rows=inputa->Rows;  
	output->Cols=inputa->Cols;  
	output->Val=(double**)malloc(output->Rows*sizeof(double*));    
	for(unsigned int i=0;i<output->Rows;i++)  
		output->Val[i]=(double*)malloc(output->Cols*sizeof(double));    
	for(unsigned int i=0;i<inputa->Rows;i++)
		for(unsigned int j=0; j<inputa->Cols;j++)
			output->Val[i][j]=inputa->Val[i][j]-inputb->Val[i][j];
	return Success;  
}

/* 
* @function: 	My_MatrixScale
* @parameter:	[in] 	*input			The first address of the input matrix.
* @parameter:	[in] 	a				The scale of the multiplication operation.
* @parameter:	[out]	*output			The first address of the output vector based on matrix Scale.
* @return:		Successes or failures of the results of the calculations.
*/
ResStatus_TypeDef My_MatrixScale(Matrix_TypeDef* input, double a, Matrix_TypeDef* output)
{
	output->Rows=input->Rows;  
	output->Cols=input->Cols;  
	output->Val=(double**)malloc(output->Rows*sizeof(double*));    
	for(unsigned int i=0;i<output->Rows;i++)  
		output->Val[i]=(double*)malloc(output->Cols*sizeof(double));    
	for(unsigned int i=0;i<input->Rows;i++)
		for(unsigned int j=0; j<input->Cols;j++)
			output->Val[i][j]=a*input->Val[i][j];
	return Success;
}

/*
* @function: 	My_MatrixMul
* @parameter:	[in]    *inputa			The first address of the input matrix.
* @parameter:	[in] 	*inputb			The first address of another input matrix.
* @parameter:	[out]	*output			The first address of the output matrix based on matrix multiplication.
* @return:		Successes or failures of the results of the calculations.
*/
ResStatus_TypeDef My_MatrixMul(Matrix_TypeDef* inputa, Matrix_TypeDef* inputb, Matrix_TypeDef* output)  
{
	if(inputa->Cols != inputb->Rows)
		return Error;
	output->Rows = inputa->Rows;  
	output->Cols = inputb->Cols;  
	output->Val = (double**)malloc(output->Rows * sizeof(double*));    
	for(unsigned int i = 0; i < output->Rows; i++)  
		output->Val[i] = (double*)malloc(output->Cols * sizeof(double));    
	for(unsigned int i = 0; i < output->Rows; i++)
		for(unsigned int j = 0; j < output->Cols; j++)
			for(unsigned int k = 0;k < inputa->Cols; k++)
				output->Val[i][j] += inputa->Val[i][k] * inputb->Val[k][j];
	return Success;
}

/*
* @function: 	My_MatrixTran
* @parameter:	[in]    *input			The first address of the input matrix.
* @parameter:	[out]	*output			The first address of the output matrix based on matrix Transpose.
* @return:		Successes or failures of the results of the calculations.
*/
ResStatus_TypeDef My_MatrixTran(Matrix_TypeDef* input, Matrix_TypeDef* output)  
{
	output->Rows=input->Cols;  
	output->Cols=input->Rows;  
	output->Val=(double**)malloc(output->Rows*sizeof(double*));    
	for(unsigned int i=0;i<output->Rows;i++)  
		output->Val[i]=(double*)malloc(output->Cols*sizeof(double));    
	for(unsigned int i=0;i<output->Rows;i++)
		for(unsigned int j=0;j<output->Cols;j++)
				output->Val[i][j]=input->Val[j][i];
	return Success;
}

/*
* @function: 	My_MatrixConv
* @parameter:	[in] 	*input			The first address of the input matrix.
* @parameter:	[in] 	*core			The first address of the convolution core matrix.
* @parameter:	[out]	*output			The first address of the output matrix based on matrix convolution.
* @return:		Successes or failures of the results of the calculations.
*/
ResStatus_TypeDef My_MatrixConv(Matrix_TypeDef* input, Matrix_TypeDef* core, Matrix_TypeDef* output)
{
	if((input->Cols+2<core->Cols)||(input->Rows+2<core->Rows))
		return Error;
	Matrix_TypeDef exinput,excore;
	exinput.Cols = input->Cols+2;
	exinput.Rows = input->Rows+2;
	exinput.Val=(double**)malloc(exinput.Rows*sizeof(double*));    
	for(unsigned int i=0;i<exinput.Rows;i++)  
		exinput.Val[i]=(double*)malloc(exinput.Cols*sizeof(double));
	for(unsigned int i=0;i<exinput.Rows;i++)
		for(unsigned int j=0;j<exinput.Cols;j++)
			if((i==0)||(j==0)||(i==exinput.Rows-1)||(j==exinput.Cols-1))
				exinput.Val[i][j]=0;
			else 
				exinput.Val[i][j]=input->Val[i-1][j-1];
	excore.Cols = core->Cols;
	excore.Rows = core->Rows;
	excore.Val = (double**)malloc(excore.Rows*sizeof(double*));    
	for(unsigned int i=0;i<excore.Rows;i++)  
		excore.Val[i]=(double*)malloc(excore.Cols*sizeof(double));
	for(unsigned int i=0;i<excore.Rows;i++)
		for(unsigned int j=0;j<excore.Cols;j++)
				excore.Val[i][j]=core->Val[excore.Rows-1-i][excore.Cols-1-j];
	output->Rows = exinput.Rows - excore.Rows + 1;
	output->Cols = exinput.Cols - excore.Cols + 1;	
	output->Val = (double**)malloc(output->Rows*sizeof(double*));    
	for(unsigned int i=0;i<output->Rows;i++)  
		output->Val[i]=(double*)malloc(output->Cols*sizeof(double));	
	for(unsigned int i=0; i<output->Rows; i++)
		for(unsigned int j=0; j<output->Cols; j++)
			for(unsigned int m=0;m<excore.Rows;m++)
				for(unsigned int n=0;n<excore.Cols;n++)
					output->Val[i][j] += excore.Val[m][n] * exinput.Val[m+i][n+j];
	My_MatrixFree(&exinput);
	My_MatrixFree(&excore);
	return Success;
}

/*
* @function: 	My_MatrixInve
* @parameter:	[in] 	*input			The first address of the input matrix.
* @parameter:	[out]	*output			The first address of the output matrix based on matrix inverse.
* @return:		Successes or failures of the results of the calculations.
*/
ResStatus_TypeDef My_MatrixInve(Matrix_TypeDef* input, Matrix_TypeDef* output) 
{
    unsigned int  k;
    double pivot, factor;
    unsigned int max_row;
    if(input->Rows != input->Cols)
        return Error;
    output->Rows = input->Rows;
    output->Cols = input->Cols;
    output->Val = (double**)malloc(output->Rows * sizeof(double*));
    double** temp = (double**)malloc(input->Rows * sizeof(double*));
    for (unsigned int i = 0; i < output->Rows; i++)
    {
        output->Val[i] = (double*)malloc(output->Rows * sizeof(double));
        temp[i] = (double*)malloc(input->Rows * sizeof(double));
        for (unsigned int j = 0; j < input->Rows; j++)
        {
            temp[i][j] = input->Val[i][j];
            output->Val[i][j] = (i==j) ? 1.0 : 0.0;
        }
    }
    for (k = 0; k < input->Rows; k++) 
    {
        max_row = k;
        pivot = My_ABS(temp[k][k]);
        for (unsigned int i = k + 1; i < input->Rows; i++) 
        {
            if (My_ABS(temp[i][k]) > pivot) 
            {
                max_row = i;
                pivot = My_ABS(temp[i][k]);
            }
        }
        if (pivot < My_ERROR) 
        {
            for (unsigned int i = 0; i < input->Rows; i++)
            {
                free(output->Val[i]);
                free(temp[i]);
            }
            free(output->Val);
            free(temp);
            return Error;
        }
        if (max_row != k) 
        {
            double* tmp_row = temp[k];
            temp[k] = temp[max_row];
            temp[max_row] = tmp_row;
            tmp_row = output->Val[k];
            output->Val[k] = output->Val[max_row];
            output->Val[max_row] = tmp_row;
        }
        pivot = temp[k][k];
        for (unsigned int j = 0; j < input->Rows; j++) 
        {
            temp[k][j] /= pivot;
            output->Val[k][j] /= pivot;
        }
        for (unsigned int i = 0; i < input->Rows; i++) 
        {
            if (i != k) 
            {
                factor = temp[i][k];
                for (unsigned int j = 0; j < input->Rows; j++) 
                {
                    temp[i][j] -= factor * temp[k][j];
                    output->Val[i][j] -= factor * output->Val[k][j];
                }
            }
        }
    }
    for (unsigned int i = 0; i < input->Rows; i++)
        free(temp[i]);
    free(temp);
    return Success;
}

/*
* @function: 	My_MatrixDet
* @parameter:	[in] 	*input			The first address of the input matrix.
* @parameter:	[out]	*output			The address of the determinant of the matrix.
* @return:		Successes or failures of the results of the calculations.
*/
ResStatus_TypeDef My_MatrixDet(Matrix_TypeDef* input, double* output) 
{
    if (input->Rows != input->Cols)
        return Error;
    double det = 1.0;
    int sign = 1;
    double** temp = (double**)malloc(input->Rows * sizeof(double*));
    for (unsigned int i = 0; i < input->Rows; i++) 
	{
        temp[i] = (double*)malloc(input->Rows * sizeof(double));
        for (unsigned int j = 0; j < input->Rows; j++)
            temp[i][j] = input->Val[i][j];
    }
    for (unsigned int k = 0; k < input->Rows; k++) 
    {
        unsigned int max_row = k;
        double max_val = My_ABS(temp[k][k]);
        for (unsigned int i = k + 1; i < input->Rows; i++) 
        {
            if (My_ABS(temp[i][k]) > max_val) 
            {
                max_row = i;
                max_val = My_ABS(temp[i][k]);
            }
        }
        if (max_val < My_ERROR) 
        {
            det = 0.0;
            break;
        }
        if (max_row != k) 
        {
            double* tmp_row = temp[k];
            temp[k] = temp[max_row];
            temp[max_row] = tmp_row;
            sign = -sign;
        }
        det *= temp[k][k];
        for (unsigned int i = k + 1; i < input->Rows; i++) 
        {
            double factor = temp[i][k] / temp[k][k];
            for (unsigned int j = k; j < input->Rows; j++)
                temp[i][j] -= factor * temp[k][j];
        }
    }
    for (unsigned int i = 0; i < input->Rows; i++)
        free(temp[i]);
    free(temp);
    *output = det * sign;
    return Success;
}

